#include "movejointoperation.h"

#include "ethernetdevice.h"

#define PI 3.1415926

MoveJointOperation::MoveJointOperation(QObject *parent)
    : Operation{parent}
    , m_pDescription((Description*)parent)
{
    m_dblJointAngles.append(0.0);
    m_dblJointAngles.append(0.0);
    m_dblJointAngles.append(0.0);
    m_dblJointAngles.append(0.0);
    m_dblJointAngles.append(0.0);
    m_dblJointAngles.append(0.0);
}

MoveJointOperation::~MoveJointOperation(){

}

void MoveJointOperation::run(){
    Device* pDevice = m_pDescription->FindDeviceById(m_strDeviceID);

    if(pDevice == NULL){
        qDebug() << "没有找到对应的设备：ID = " << m_strDeviceID;
        return ;
    }
    QList<QVariant> variants;
    for(int i = 0; i < m_dblJointAngles.size(); i++){
        variants.append(QVariant(m_dblJointAngles[i]));
    }

    qDebug() << "MoveJointOperation::run";
    variants.append(QVariant(m_dblVelocity));
    variants.append(QVariant(m_dblAcceleration));
    pDevice->writeData(variants);
}

QList<double>& MoveJointOperation::dblJointAngles()
{
    return m_dblJointAngles;
}

void MoveJointOperation::setDblJointAngles(const QList<double> &newDblJointAngles)
{
    m_dblJointAngles = newDblJointAngles;
}

QString MoveJointOperation::getClassName(){
    return this->metaObject()->className();
}

void MoveJointOperation::writeXML(QXmlStreamWriter& writer){
    writer.writeStartElement("MoveJointOperation");
    writer.writeAttribute("device", m_strDeviceID);
    int index = 1;
    foreach(double value, m_dblJointAngles){
        writer.writeTextElement(QString("joint%1").arg(index++), QString::number(value));
    }

    writer.writeEndElement();
}

void MoveJointOperation::readFromXML(QXmlStreamReader& reader){
    QXmlStreamAttributes attributes = reader.attributes();
    if(attributes.hasAttribute("device")) {
        m_strDeviceID = attributes.value("device").toString();
    }
    if(attributes.hasAttribute("velocity")) {
        m_dblVelocity = attributes.value("velocity").toDouble();
    }
    if(attributes.hasAttribute("acceleration")) {
        m_dblAcceleration = attributes.value("acceleration").toDouble();
    }

    m_dblJointAngles.clear();
    QXmlStreamReader::TokenType token = reader.readNext();
    while (!(reader.tokenType() == QXmlStreamReader::EndElement &&
             reader.name() == QStringLiteral("MoveJointOperation"))) {

        if (reader.name() == QStringLiteral("joint1") && reader.isStartElement()) {
            m_dblJointAngles.append(reader.readElementText().toDouble());
        }else if (reader.name() == QStringLiteral("joint2") && reader.isStartElement()) {
            m_dblJointAngles.append(reader.readElementText().toDouble());
        }else if (reader.name() == QStringLiteral("joint3") && reader.isStartElement()) {
            m_dblJointAngles.append(reader.readElementText().toDouble());
        }else if (reader.name() == QStringLiteral("joint4") && reader.isStartElement()) {
            m_dblJointAngles.append(reader.readElementText().toDouble());
        }else if (reader.name() == QStringLiteral("joint5") && reader.isStartElement()) {
            m_dblJointAngles.append(reader.readElementText().toDouble());
        }else if (reader.name() == QStringLiteral("joint6") && reader.isStartElement()) {
            m_dblJointAngles.append(reader.readElementText().toDouble());
        }

        reader.readNext();
    }
}
